home *** CD-ROM | disk | FTP | other *** search
/ Aminet 6 / Aminet 6 - June 1995.iso / Aminet / misc / sci / RARS_Amiga_2.lha / RARS / cntrl1.cpp < prev    next >
Encoding:
C/C++ Source or Header  |  1995-03-31  |  5.0 KB  |  124 lines
  1. // CNTRL1.CPP - "driver" function for RARS - M. Timin, Dec. 1994
  2. // (see CNTRL0.CPP for better comments)
  3. // adapted to ver. 0.39 3/6/95 by M. Timin
  4.  
  5. #include <string.h>
  6. #include <stdlib.h>
  7. #include <math.h>
  8. #include "car.h"
  9.  
  10. extern char* glob_name;       // The name string, below, will be copied here
  11. extern const double CARWID;   // car width, feet
  12.  
  13. // The task of this function is to compute vc and alpha.  vc is the speed
  14. // of the bottom of the wheel relative to the car. (Velocity Commanded)
  15. // If the car is coasting at zero power then vc == s.v, the speed of the
  16. // car.  If vc is held constant for a while the cars speed will approach
  17. // vc, unless it is cornering, in which case vc may greatly exceed the
  18. // speed.  The angle "alpha" is the angle between the car's orientation angle
  19. // and its velocity vector.  (like angle of attack of an aircraft)
  20. // Cornering force depends on alpha.  (It is also thought of as a slip angle.)
  21. con_vec cntrl1(situation s)
  22. {
  23.    const char name[] = "Knuth";      // This is the robot driver's name!
  24.    static int init_flag = 1;           // cleared by first call
  25.    // A "lane" is just a preferred distance from the wall of the track.
  26.    static double lane = 75;    // determines right-left position on straightaway
  27.    static int lane_time = 0;           // counts time when not in normal lane
  28.    con_vec result;                     // This is what is returned.
  29.    double width;                      // track width, feet
  30.    const double normalane = 75.0;      // usual position down the straight
  31.    const double steer_gain = .18;      // servo parameters on the straight:
  32.    const double steer_damp = .3;
  33.    double corn_speed;     // target speed for cornering, ft/sec
  34.    double alpha, vc;           // components of result
  35.    static double power_control = 1.0;
  36.    static double slip_var = 90.0;
  37.    const double init_slip_var = 90.0;
  38.    double temp;
  39.  
  40.    if(init_flag)  {            // first time through, only copy name:
  41.       strcpy(glob_name, name);
  42.       init_flag = 0;
  43.       result.alpha = result.vc = 0;
  44.       return result;
  45.    }
  46.  
  47.   if(stuck(s.backward, s.v,s.vn, s.to_lft,s.to_rgt, &result.alpha,&result.vc))
  48.       return result;
  49.  
  50.    // power_control is recent past average of power_req
  51.    power_control += .175 * (s.power_req - power_control);
  52.    slip_var -= 4.0 * (power_control - 1.0);
  53.  
  54.    if(s.cur_rad > 0.0)
  55.       corn_speed = 61.3 * sqrt(s.cur_rad/100.0);
  56.    else if(s.cur_rad < 0.0)
  57.       corn_speed = 61.3 * sqrt((-s.cur_rad)/100.0);
  58.    else corn_speed = 61.3;
  59.  
  60.    // maybe choose a different lane: (to help in passing)
  61.    if(!s.dead_ahead) {
  62.       if(lane_time <= 0) {
  63.          lane = normalane;
  64.          lane_time = 0;
  65.       }
  66.    }
  67.    else if(!lane_time)   {
  68.       // pick a different lane:
  69.       if(lane >= 80)               // pick a new lane somehow:
  70.          lane = random(60) - 20;
  71.       else if(lane <= -80)
  72.          lane = 20 - random(60);
  73.       else
  74.          lane = random(180) - 90;
  75.       lane_time = 160;              // we will stay in the new lane this long
  76.    }
  77.    if(lane_time > 0)
  78.       --lane_time;
  79.  
  80.    // set alpha based on a servo-mechanism approach, trying to maintain
  81.    // a certain distance from the track wall:
  82.    width = s.to_lft + s.to_rgt;                        // find width of track
  83.    if(s.cur_rad == 0)  {     // If we are on the straightaway:
  84.       // alpha is proportional to lane error:
  85.       alpha = .35 * steer_gain * (s.to_lft - s.to_rgt - lane) / width;
  86.       if(s.dead_ahead)       // change lanes quicker if someone in front
  87.           alpha *= 2.0;
  88.    }
  89.    else if(s.cur_rad > 0) {  // in the turn, stick close to the inside:
  90.       alpha = steer_gain * (4 * (s.to_lft-2.5*CARWID) / width
  91.                                                        + .2 * width/s.to_rgt);
  92.       if(s.dead_ahead)    // if someone ahead, reduce alpha to pass on outside
  93.           alpha *= .8;
  94.    }
  95.    else {                   // this is for right turns, similar to above
  96.       alpha = -steer_gain * (4 * (s.to_rgt-2.5*CARWID) / width
  97.                                                         + .2 * width/s.to_lft);
  98.       if(s.dead_ahead)
  99.           alpha *= .8;
  100.    }
  101.    alpha -= steer_damp * s.vn / s.v;  // This is damping, to prevent oscillation
  102.  
  103.    if(s.cur_rad == 0)         // If we are on a straightaway,
  104.       if(s.to_end/s.cur_len > .32)      // if we are far from the end:
  105.          vc = s.v + slip_var/s.v;        // keep accellerating near full power
  106.       else  {                 // otherwise,
  107.          // decelerate, approaching cornering speed:
  108.          vc = .1 * (9*s.v + corn_speed);
  109.          // when near end of straight, steer toward the turn (assumed left!)
  110.          temp = .55 * (s.v/corn_speed) *
  111.                                 (s.cur_len/(s.to_end + s.cur_len) - (1/1.32));
  112.          alpha += temp * (lane + 90.0) / 180.0;
  113.          if(s.dead_ahead)    // cut to left if someone's dead ahead!
  114.              alpha *= 2.0;
  115.       }
  116.    else     {       // if we're in the curve, maintain speed, +
  117.       vc = .5*(s.v+corn_speed) + .9 / (s.to_end + .4);
  118.       slip_var = init_slip_var;
  119.    }
  120.  
  121.    result.vc = vc;   result.alpha = alpha;
  122.    return result;
  123. }
  124.